#include "rclcpp/rclcpp.hpp"
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <thread>

#include "cartographer_ros_msgs/msg/landmark_list.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
// #include <tf2/LinearMath/Quaternion.h>
using namespace std::chrono_literals;

class DmReader : public rclcpp::Node
{
public:
    DmReader() : Node("dm_reader")
    {
        // 参数声明
        declare_parameter<int>("port", 5004);
        declare_parameter<std::string>("server_ip", "192.168.16.4");
        declare_parameter<int>("buffer_size", 1024);
        declare_parameter<double>("translation_weight", 50.0);
        declare_parameter<double>("rotation_weight", 50.0);

        // 创建发布器
        qr_pose_pub_ = create_publisher<cartographer_ros_msgs::msg::LandmarkList>("DM_pose", 10);

        // 启动TCP服务器线程
        server_thread_ = std::thread(&DmReader::run_tcp_server, this);
        RCLCPP_INFO(get_logger(), "DM Reader initialized");
    }

    ~DmReader()
    {
        if (server_thread_.joinable())
        {
            server_thread_.join();
        }
    }

private:
    std::thread server_thread_;
    rclcpp::Publisher<cartographer_ros_msgs::msg::LandmarkList>::SharedPtr qr_pose_pub_;

    void run_tcp_server()
    {

        int port = this->get_parameter("port").as_int();
        std::string server_ip = this->get_parameter("server_ip").as_string();
        int buffer_size = this->get_parameter("buffer_size").as_int();

        int server_fd = socket(AF_INET, SOCK_STREAM, 0);
        sockaddr_in server_addr{};
        server_addr.sin_family = AF_INET;
        server_addr.sin_port = htons(port);
        inet_pton(AF_INET, server_ip.c_str(), &server_addr.sin_addr);

        bind(server_fd, (sockaddr *)&server_addr, sizeof(server_addr));
        listen(server_fd, 3);

        while (rclcpp::ok())
        {
            sockaddr_in client_addr{};
            socklen_t addr_len = sizeof(client_addr);
            int client_fd = accept(server_fd, (sockaddr *)&client_addr, &addr_len);

            uint8_t buffer[21];
            ssize_t bytes_read = recv(client_fd, buffer, sizeof(buffer), 0);

            if (bytes_read > 0)
            {
                std::vector<uint8_t> data(buffer, buffer + bytes_read);
                process_data(data);
            }

            close(client_fd);
        }
        close(server_fd);
    }

    void process_data(const std::vector<uint8_t> &data)
    {
        double translation_weight = this->get_parameter("translation_weight").as_double();
        double rotation_weight = this->get_parameter("rotation_weight").as_double();
        if (data.size() < 21)
        {
            // RCLCPP_ERROR("Received data length is too short for parsing.");
            return;
        }

        uint8_t result = data[0];
        double x_offset, y_offset, yaw;
        int tag_number;
        for (size_t i = 1; i <= 19; i++)
        {
            result ^= data[i];
        }

        bool on_qrcode = (data[1] & 0x40) == 0x40;
        unsigned int x_position = (data[2] * 0x80 * 0x4000) + (data[3] * 0x4000) + (data[4] * 0x80) + data[5];
        if (x_position > 0x800000)
        {
            x_position = (0x1000000 - x_position);
            x_offset = -static_cast<double>(x_position) / 10000;
        }
        else
        {
            x_offset = static_cast<double>(x_position) / 10000;
        }
        unsigned int y_position = (data[6] * 0x80) + data[7];
        if (y_position > 0x2000)
        {
            y_position = (0x4000 - y_position);
            y_offset = -static_cast<double>(y_position) / 10000;
        }
        else
        {
            y_offset = static_cast<double>(y_position) / 10000;
        }

        unsigned int angle = (data[10] * 0x80) + data[11];
        yaw = static_cast<double>(angle) / 10;
        // 传感器的输出yaw是以Z朝向地面的
        // 输入landmark的四元数中，Z轴是朝向下的，刚好传感器本身Z轴也是朝下的
        // yaw = 360 - yaw;  // 通过360 - yaw 可以等价将角度的方向转换成Z朝上
        if (yaw > 180.0)
        {
            yaw = yaw - 360;
        }
        yaw = yaw * (M_PI / 180.0);

        int raw_tag_number = (data[14] * 0x80 * 0x4000) + (data[15] * 0x4000) + (data[16] * 0x80) + data[17];
        if (raw_tag_number < 0x3FFF)
        {
            raw_tag_number = (data[16] * 0x80) + data[17];
        }
        tag_number = static_cast<int>(raw_tag_number);

        if (result == data[20])
        {
            if (on_qrcode)
            {
                // 将yaw转换为四元数
                geometry_msgs::msg::Quaternion orientation;
                orientation.x = 0;
                orientation.y = 0;
                orientation.z = std::sin(yaw / 2.0);
                orientation.w = std::cos(yaw / 2.0);

                // 发布ROS消息
                auto qrPoseList = cartographer_ros_msgs::msg::LandmarkList();
                auto qrPoseMsg = cartographer_ros_msgs::msg::LandmarkEntry();
                qrPoseMsg.id = "landmark2" + std::to_string(tag_number);
                qrPoseMsg.translation_weight = translation_weight;
                qrPoseMsg.rotation_weight = rotation_weight;
                qrPoseMsg.tracking_from_landmark_transform.position.x = x_offset;
                qrPoseMsg.tracking_from_landmark_transform.position.y = y_offset;
                qrPoseMsg.tracking_from_landmark_transform.position.z = 0.0;
                qrPoseMsg.tracking_from_landmark_transform.orientation = orientation;
                qrPoseList.landmarks.push_back(qrPoseMsg);
                qr_pose_pub_->publish(qrPoseList);
            }
        }
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<DmReader>());
    rclcpp::shutdown();
    return 0;
}
